#include "rclcpp/rclcpp.hpp"
#include <fstream>
#include <iostream>
#include <lib/md5.h>
#include "lib/base64.h"
#include <std_msgs/msg/int8.hpp>
#include <agv_srvs/srv/firmware_update.hpp>
#include <agv_srvs/srv/ftp_request.hpp>
#include <agv_srvs/srv/version.hpp>

// ros::Publisher pub;
rclcpp::Client<agv_srvs::srv::FirmwareUpdate>::SharedPtr client1;
rclcpp::Client<agv_srvs::srv::Version>::SharedPtr client2;
rclcpp::Client<agv_srvs::srv::FtpRequest>::SharedPtr client3;

std::string filePath;
std::string fileName;

// void test()
// {
//     std::string fullName = filePath + "/" + fileName;
//     agv_srvs::srv::FirmwareUpdate fw;
//     RCLCPP_INFO(rclcpp::get_logger("Firmware"), "file:%s", fullName.c_str());
//     fw.Request.fw_name = fileName;

//     std::ifstream in(fullName.c_str(), std::ifstream::binary);

//     if (!in.is_open()) {
//         RCLCPP_INFO(rclcpp::get_logger("Firmware"), "file open error");
//         return;
//     }
//     in.seekg(0, std::ios_base::end);
//     fw.Request.fw_size = in.tellg();
//     RCLCPP_INFO(rclcpp::get_logger("Firmware"), "file size:%d", fw.Request.fw_size);
//     in.seekg(0, std::ios_base::beg);
//     std::istreambuf_iterator<char> beg(in), end;
//     std::string content;
//     content.assign(beg, end);
//     in.close();

//     std::string m = md5(content);
//     fw.Request.fw_md5 = m;

//     RCLCPP_INFO(rclcpp::get_logger("Firmware"), "MD5 Sum:%s", m.c_str());

//     // std::string encoded = base64_encode(reinterpret_cast<const unsigned char*>(content.data()), content.length());
//     // fw.Request.fw_content = encoded;

//     if (client1.call(fw)) {
//         RCLCPP_INFO(rclcpp::get_logger("Firmware"), "return %d:%s", fw.response.success, fw.response.message.c_str());
//     } else {
//         RCLCPP_ERROR(rclcpp::get_logger("Firmware"), "Failed to call service fw_update:%d:%s", fw.response.success, fw.response.message.c_str());
//     }
// }

int main(int argc, char **argv){
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("test");

    node->get_parameter_or("file_path", filePath, std::string(getenv("HOME")));
    RCLCPP_INFO_STREAM(rclcpp::get_logger("Firmware"), "filePath: "<<filePath);
    node->get_parameter_or("file_name", fileName, std::string("lgimRobot.tar.gz"));
    RCLCPP_INFO_STREAM(rclcpp::get_logger("Firmware"), "fileName: "<<fileName);

    auto sub = node->create_subscription<std_msgs::msg::Int8>(
        "/test", 10, 
        [node](const std_msgs::msg::Int8::SharedPtr msg) {
            if(msg->data == 0) {
                // test();
            } else if (msg->data == 1) {
                auto request = std::make_shared<agv_srvs::srv::Version::Request>();
              
                try {

                    using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::Version>::SharedFuture;
                    auto response_received_callback = [](ServiceResponseFuture future) {
                        RCLCPP_INFO(rclcpp::get_logger("Firmware"), 
                            "Got result: %s:%s:%s", future.get()->ver.c_str(), future.get()->branch.c_str(), future.get()->hash.c_str());
                    };

                    auto result = client2->async_send_request(request, response_received_callback);

                    // RCLCPP_INFO(rclcpp::get_logger("Firmware"), "send version request...");
                    // if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
                    //     RCLCPP_INFO(rclcpp::get_logger("Firmware"), "return %s:%s:%s", result.get()->ver.c_str(), result.get()->branch.c_str(), result.get()->hash.c_str());
                    // } else {
                    //     RCLCPP_ERROR(rclcpp::get_logger("Firmware"), "Failed to call service get version");

                    // }
              
                } catch (const std::exception &e) {
                    RCLCPP_ERROR(rclcpp::get_logger("Firmware"), "error: %s", e.what());
                }

            } else {
                // agv_srvs::srv::FtpRequest ftp;
                // if (client3.call(ftp)) {
                //     RCLCPP_INFO(rclcpp::get_logger("Firmware"), "return %d:%s", ftp.response.success, ftp.response.message.c_str());
                // } else {
                //     RCLCPP_ERROR(rclcpp::get_logger("Firmware"), "failed to ftp Request.");
                // }
            }
            return 0;
        });

    client1 = node->create_client<agv_srvs::srv::FirmwareUpdate>("/fw_update");
    while (!client1->wait_for_service(std::chrono::seconds(1))) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(rclcpp::get_logger("Firmware"), "Interrupted while waiting for the service. Exiting.");
            return 0;
        }
        RCLCPP_INFO(rclcpp::get_logger("Firmware"), "service not available, waiting again...");
    }

    client2 = node->create_client<agv_srvs::srv::Version>("/fw_version");
    while (!client2->wait_for_service(std::chrono::seconds(1))) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(rclcpp::get_logger("Firmware"), "Interrupted while waiting for the service. Exiting.");
            return 0;
        }
        RCLCPP_INFO(rclcpp::get_logger("Firmware"), "service not available, waiting again...");
    }

    client3 = node->create_client<agv_srvs::srv::FtpRequest>("/ftp_Request");


    // test();
    RCLCPP_INFO(rclcpp::get_logger("Firmware"), "test start.");
    rclcpp::spin(node);
    RCLCPP_INFO(rclcpp::get_logger("Firmware"), "test end.");
    return 0;
}
